/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.archwood.frc2607;


import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DriverStationLCD;
import edu.wpi.first.wpilibj.DriverStationLCD.Line;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import edu.wpi.first.wpilibj.image.BinaryImage;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.CriteriaCollection;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.ParticleAnalysisReport;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class ReboundRumblePrototype extends IterativeRobot
{
    LogomotionDriveTrain headbobbingmovement;
    Joystick joy, joy2, joy3;
    SpeedController Roller, Feeder, Shooter, Rotator;
    DriverStationLCD lcd = DriverStationLCD.getInstance();
    AxisCamera camera;
    CriteriaCollection cc;
    Relay LEDs;
    int shooterCharge = 0;
    int imagerefreshRate = 0;
    public void robotInit()
    {
        try
        {
            System.out.println("Drive");
            headbobbingmovement = new LogomotionDriveTrain();
            System.out.println("J1");
            joy = new Joystick(1);
            System.out.println("J2");
            joy2 = new Joystick(2);
            System.out.println("J3");
            joy3 = new Joystick(3);
            System.out.println("Roll");
            Roller = new CANJaguar(14);
            System.out.println("Feed");
            Feeder = new CANJaguar(8);
            System.out.println("Shot");
            Shooter = new CANJaguar(20);
            System.out.println("Rotate");
            Rotator = new Victor(2);
            System.out.println("LEDs");
            LEDs = new Relay(1,Relay.Direction.kForward);
            Timer.delay(8);
            System.out.println("Camera");
            camera = AxisCamera.getInstance();
            camera.writeResolution(AxisCamera.ResolutionT.k320x240);
            cc = new CriteriaCollection();
            System.out.println("RES:"+camera.getResolution().height);
        }
        catch (CANTimeoutException ex)
        {
            ex.printStackTrace();
        }
    }
    public void showMessage(String msg, int line)
    {
        Line l = DriverStationLCD.Line.kMain6;
        switch (line)
        {
            case 1:
                l=DriverStationLCD.Line.kUser2;
            break;
            case 2:
                l=DriverStationLCD.Line.kUser3;
            break;
            case 3:
                l=DriverStationLCD.Line.kUser4;
            break;
            case 4:
                l=DriverStationLCD.Line.kUser5;
            break;
            case 5:
                l=DriverStationLCD.Line.kUser6;
            break;
        }
        lcd.println(l,1,msg+"                           ");
        lcd.updateLCD();
    }
    public void autonomousPeriodic()
    {

    }
    public void teleopPeriodic() {
        showMessage(""+((-joy2.getZ()+1)/2),1);
        headbobbingmovement.mecanumDrive2(joy.getY(), -joy.getX(),-joy.getRawAxis(3));
        if (joy2.getRawButton(4))
        {
            Rotator.set(.3);
        }
        else
        {
            if (joy2.getRawButton(5))
            {
                Rotator.set(-.3);
            }
            else
            {
                Rotator.set(0);
            }
        }
        if (joy2.getRawButton(1))
        {
            if (joy2.getRawButton(3))
            {
                Shooter.set(.8d);
            }
            else
            {
                 Shooter.set((-joy2.getZ()+1)/2);
            }
            shooterCharge++;
            if (shooterCharge>=250)
            {
                shooterCharge=251;
                Feeder.set(-.5);
            }
        }
        else
        {
            shooterCharge=0;
            Shooter.set(0);
            Feeder.set(0);
        }
        if (joy2.getRawButton(2))
        {
            Roller.set(-1);
        }
        else
        {
            if (joy2.getRawButton(8))
            {
                Roller.set(1);
            }
            else
            {
                Roller.set(0);
            }
        }
        if (joy3.getRawButton(1))
        {
           LEDs.set(Relay.Value.kOn);
        }
        else
        {
            LEDs.set(Relay.Value.kOff);
        }
        if (joy3.getRawButton(1))
        {
             ColorImage image;
             try
             {
                image = camera.getImage();
                BinaryImage thresholdImage = image.thresholdRGB(150, 255, 00, 180, 0, 180);   // keep only green objects
                BinaryImage bigObjectsImage = thresholdImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage convexHullImage = bigObjectsImage.convexHull(false);          // fill in occluded rectangles
                BinaryImage goodimage = convexHullImage.removeSmallObjects(false, 2);  // remove small artifacts
                BinaryImage filteredImage = goodimage.particleFilter(cc);           // find filled in rectangles
                ParticleAnalysisReport[] reports = filteredImage.getOrderedParticleAnalysisReports();  // get list of result
                if (reports.length>0)
                {
                    int biggestSize=320;
                    ParticleAnalysisReport r = reports[0];
                    if (joy3.getRawButton(4))
                    for (int i = 0; i < reports.length; i++)
                    {
                        if (reports[i].center_mass_x<biggestSize)
                        {
                            biggestSize=reports[i].center_mass_x;
                            r = reports[i];
                        }
                    }
                    if (joy3.getRawButton(3))
                    for (int i = 0; i < reports.length; i++)
                    {
                        if (reports[i].center_mass_y<biggestSize)
                        {
                            biggestSize=reports[i].center_mass_y;
                            r = reports[i];
                        }
                    }
                    if (joy3.getRawButton(5))
                    for (int i = 0; i < reports.length; i++)
                    {
                        if (reports[i].center_mass_x>biggestSize)
                        {
                            biggestSize=reports[i].center_mass_x;
                            r = reports[i];
                        }
                    }
                    if (joy3.getRawButton(2))
                    for (int i = 0; i < reports.length; i++)
                    {
                        if (reports[i].center_mass_y>biggestSize)
                        {
                            biggestSize=reports[i].center_mass_y;
                            r = reports[i];
                        }
                    }
                    System.out.println("I found something!");
                    System.out.println("Center of Mass:"+(r.center_mass_x));
                    showMessage(""+r.imageHeight,5);
                    if (r.center_mass_x>150)
                    {
                        if (r.center_mass_x<170)
                        {
                            Rotator.set(0);
                            System.out.println("LOCKED ON!");
                        }
                        else
                        {
                        Rotator.set(-.45);
                        }
                    }
                    else
                    {
                        if (r.center_mass_x<170)
                        {
                            Rotator.set(.45);
                        }
                        else
                        {
                            Rotator.set(0);
                            System.out.println("LOCKED ON!");
                        }
                    }
                }
                else
                {
                    System.out.println("I'M BLIND!");
                }
                filteredImage.free();
                convexHullImage.free();
                bigObjectsImage.free();
                thresholdImage.free();
                goodimage.free();
                image.free();
            }
            catch (AxisCameraException ex) {
                ex.printStackTrace();
            }
            catch (NIVisionException ex) {
                ex.printStackTrace();
            }
            }
    }
}